import struct
import matplotlib
from socket import *  
import numpy as np 
matplotlib.use('TkAgg')
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg 
from matplotlib.figure import Figure
from Tkinter import *
import datetime
import time
import thread
from matplotlib.ticker import MultipleLocator, FormatStrFormatter

class ZoomPan:
    def __init__(self):
        self.press = None
        self.cur_xlim = None
        self.cur_ylim = None
        self.x0 = None
        self.y0 = None
        self.x1 = None
        self.y1 = None
        self.xpress = None
        self.ypress = None
 
 
    def zoom_factory(self, ax, base_scale = 2.):
        def zoom(event):
            cur_xlim = ax.get_xlim()
            cur_ylim = ax.get_ylim()
 
            xdata = event.xdata # get event x location
            ydata = event.ydata # get event y location
 
            if event.button == 'down':
                # deal with zoom in
                scale_factor = 1 / base_scale
            elif event.button == 'up':
                # deal with zoom out
                scale_factor = base_scale
            else:
                # deal with something that should never happen
                scale_factor = 1
                print event.button
 
            new_width = (cur_xlim[1] - cur_xlim[0]) * scale_factor
            new_height = (cur_ylim[1] - cur_ylim[0]) * scale_factor
 
            relx = (cur_xlim[1] - xdata)/(cur_xlim[1] - cur_xlim[0])
            rely = (cur_ylim[1] - ydata)/(cur_ylim[1] - cur_ylim[0])
 
            ax.set_xlim([xdata - new_width * (1-relx), xdata + new_width * (relx)])
            ax.set_ylim([ydata - new_height * (1-rely), ydata + new_height * (rely)])
            ax.figure.canvas.draw()
 
        fig = ax.get_figure() # get the figure of interest
        fig.canvas.mpl_connect('scroll_event', zoom)
 
        return zoom
 
    def pan_factory(self, ax):
        def onPress(event):
            if event.inaxes != ax: return
            self.cur_xlim = ax.get_xlim()
            self.cur_ylim = ax.get_ylim()
            self.press = self.x0, self.y0, event.xdata, event.ydata
            self.x0, self.y0, self.xpress, self.ypress = self.press
 
        def onRelease(event):
            self.press = None
            ax.figure.canvas.draw()
 
        def onMotion(event):
            if self.press is None: return
            if event.inaxes != ax: return
            dx = event.xdata - self.xpress
            dy = event.ydata - self.ypress
            self.cur_xlim -= dx
            self.cur_ylim -= dy
            ax.set_xlim(self.cur_xlim)
            ax.set_ylim(self.cur_ylim)
 
            ax.figure.canvas.draw()
 
        fig = ax.get_figure() # get the figure of interest
 
        # attach the call back
        fig.canvas.mpl_connect('button_press_event',onPress)
        fig.canvas.mpl_connect('button_release_event',onRelease)
        fig.canvas.mpl_connect('motion_notify_event',onMotion)
 
        #return the function
        return onMotion

class UDPSocketClient:
    def __init__(self):
        self.mHost = '192.168.1.6'
        self.mPort = 6000 
        self.mBufsize = 4*1024 + 12 # 32k + 12 
        self.mAddress = (self.mHost, self.mPort)
        self.mUDPClient = socket(AF_INET, SOCK_DGRAM)
        self.mData = None
        self.mUDPClient.settimeout(5)

    def sendData(self):
        self.mUDPClient.sendto(self.mData,self.mAddress)

    def receiveData(self):
       self.mData, self.mAddress = self.mUDPClient.recvfrom(self.mBufsize)
       return self.mData
       
def bumatoyuanma(x):
  for i in range(len(x)):   
    if (x[i] > 32767):
      x[i]= x[i]-65536
      
  return x

def bumatoyuanmaSingle(x):
  # if (x > 32768):
      # x = -((x ^ 65535) + 1)    
  # elif (x == 32768):
      # x = 0 
      
  if (x > 32767): 
     x = x - 65536 
      
  return x
     
def receiveData ():
    data, addr = udpCliSock.recvfrom(4108)
    print data
    return data 

def readData(datapath):
  iData = []
  for line in open(datapath):
    if len(iData) == 4096:
      break
    length = len(line)
    iData.append(int(line))
  return (iData)
  
def sendcommand(cmdid,status,msgid,len,type,offset):
  cmdid=struct.pack('H',htons(cmdid))
  status=struct.pack('H',status)
  msgid=struct.pack('H',htons(msgid))
  len=struct.pack('H',len)
  type=struct.pack('H',type)
  offset=struct.pack('H',offset)
  data = cmdid + status + msgid + len + type + offset
  
  #udpSocketClient = UDPSocketClient()
  udpSocketClient.mData = data
  udpSocketClient.sendData()
  
def parseData(data, withHead):
  if (withHead):
    cmdid = struct.unpack('H',data[0:2])
    #print "cmd id: ", hex(ntohs(cmdid[0]))
    status = struct.unpack('H',data[2:4])
    #print "status: ",hex(ntohs(status[0]))
    msgid = struct.unpack('H',data[4:6])
    #print "msgid: ", hex(ntohs(msgid[0]))
    length = struct.unpack('H',data[6:8])
    #print "length: ",hex(ntohs(length[0]))
    type = struct.unpack('H',data[8:10])
    #print "type: ",hex(ntohs(type[0]))
    offset = struct.unpack('H',data[10:12])
    #print "offset: ",hex(ntohs(offset[0]))
    
    newdata = data[12:]

  # Parse Payload...
  
  #offsetRegTuple, offsetRegValueTuple, offsetReg, offsetRegValue
  aiData = []
  aqData = []
  biData = []
  bqData = []
  # if( msgid == 0x5a01 ): # read status
    # # Register1 offset	Value of Register1
    # offsetRegTuple = struct.unpack('H',data[:2])
    # offsetReg = int(ntohs(offsetRegTuple[0]))
    # offsetRegValueTuple = struct.unpack('H',data[2:4])
    # offsetRegValue = int(ntohs(offsetRegValueTuple[0]))
  # elif( msgid == 0x5a04 ): # Read data

  # Save the original payload
  # now = datetime.datetime.now()
  # currentTime = now.strftime('%Y-%m-%d-%H-%M-%S') 
  # rawFileName = "RawIQ-" + currentTime + ".dat"
  # rawFile=open(rawFileName,'wb')
  # rawFile.write(newdata)
  # rawFile.close()

  # 32*1024 bytes,  AIQ BIQ4 Byes I Data, 4 Bytes Q data
  for pos in xrange(0,4*1024,32):
    line = newdata[pos:pos+32]
    newline = ''
    for i in xrange(0, 32, 2):
      newline =  newline + ("%04x" % int(struct.unpack('H',line[i:i+2])[0]))
    line = newline
    newline = ''
    
    #work well for hex
    for i in range(0, len(line)+1)[::-2]:    
      newline = newline + line[i-2:i]
    line = newline
    
    # Lane#1/2
    i = 0
    aiData.append(bumatoyuanmaSingle(int(line[0:2] + line[8:10], 16)))
    aiData.append(bumatoyuanmaSingle(int(line[2:4] + line[10:12], 16)))
    aiData.append(bumatoyuanmaSingle(int(line[4:6] + line[12:14], 16)))
    aiData.append(bumatoyuanmaSingle(int(line[6:8] + line[14:16], 16)))
    # Lane#3/4
    i = 16
    aqData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    i = 18
    aqData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    i = 20
    aqData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    i = 22
    aqData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    
    # Lane#5/6
    i= 32
    biData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    i = 34
    biData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    i = 36
    biData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    i = 38
    biData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    
    # Lane#7/8
    i= 48
    bqData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    i = 50
    bqData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    i = 52
    bqData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))
    i = 54
    bqData.append(bumatoyuanmaSingle(int(line[i:i+2] + line[i+8:i+10], 16)))

  #return (aiData,aqData,biData,bqData,offsetReg,offsetRegValue)
  
  # Save the IQ data 
  # aiFileName = "AI-" + currentTime + ".txt"
  # aqFileName = "AQ-" + currentTime + ".txt"
  # biFileName = "BI-" + currentTime + ".txt"
  # bqFileName = "BQ-" + currentTime + ".txt"
  # aiFile=open(aiFileName,'wb')
  # aqFile=open(aqFileName,'wb')
  # biFile=open(biFileName,'wb')
  # bqFile=open(bqFileName,'wb')
  
  # # Save the IQ Data
  # for i in range(len(aiData)):
     # aiFile.write(str(aiData[i]))
     # aiFile.write('\n');    
     # aqFile.write(str(aqData[i]))
     # aqFile.write('\n')
     # biFile.write(str(biData[i]))
     # biFile.write('\n');    
     # bqFile.write(str(bqData[i]))
     # bqFile.write('\n')

  # aiFile.close()
  # aqFile.close()
  # biFile.close()
  # bqFile.close()
  
  return (aiData,aqData,biData,bqData)

threadRunning = True

saveLastData = False

def readanddraw():
  print  readanddraw.__name__
  
  statusVal.set(1)  
  global threadRunning
  while threadRunning:
    aiData = []
    aqData = []
    aiData = readData("AI-2018-01-22-21-04-40.txt")
    aqData = readData("AQ-2018-01-22-21-04-40.txt")

    drawPicture(aiData,aqData)
    
    biData = []
    bqData = []

    biData = readData("BI-2018-01-22-21-04-40.txt")
    bqData = readData("BQ-2018-01-22-21-04-40.txt")

    drawPicture(biData,bqData)
 
  # Save last data
  if (saveLastData):
    print "Save the last data..."
    # Save the IQ data 
    now = datetime.datetime.now()
    currentTime = now.strftime('%Y-%m-%d-%H-%M-%S')
    aiFileName = "AI-" + currentTime + ".txt"
    aqFileName = "AQ-" + currentTime + ".txt"
    biFileName = "BI-" + currentTime + ".txt"
    bqFileName = "BQ-" + currentTime + ".txt"
    aiFile=open(aiFileName,'wb')
    aqFile=open(aqFileName,'wb')
    biFile=open(biFileName,'wb')
    bqFile=open(bqFileName,'wb')
    
    # Save the IQ Data
    for i in range(len(aiData)):
       aiFile.write(str(aiData[i]))
       aiFile.write('\n');    
       aqFile.write(str(aqData[i]))
       aqFile.write('\n')
       biFile.write(str(biData[i]))
       biFile.write('\n');    
       bqFile.write(str(bqData[i]))
       bqFile.write('\n')

    aiFile.close()
    aqFile.close()
    biFile.close()
    bqFile.close()
  

def stopCapture():
  print "Stop Capture..."
  
  global threadRunning
  threadRunning = False
  
  global saveLastData
  saveLastData = True

def drawPicture(y1, y2):
  f_plot1.clear()
  f_plot1.set_title('Signal')
  f_plot1.set_xlabel('Freqs(HZ)')
  f_plot1.set_ylabel('dBm')
  f_plot1.set_ylim(-90,10)
  ymajorLocator = MultipleLocator(10) 
  yminorLocator = MultipleLocator(5) 
  xmajorLocator = MultipleLocator(0.1*1e9)
  f_plot1.yaxis.set_major_locator(ymajorLocator)
  f_plot1.yaxis.set_minor_locator(yminorLocator)
  f_plot1.xaxis.set_major_locator(xmajorLocator)
  f_plot1.yaxis.grid(True, which='minor')
  
  y = []
  for i in range(len(y1)):
    y.append(complex(y1[i],y2[i]))
  
  #y = y/32768
  yLength = len(y)
  # hanning window
  hanning_window = np.hanning(yLength)
  # total = 0.0
  # for i in range(len(hanning_window)):
    # total += hanning_window[i]
  #hanning_window = hanning_window/total
  xf = np.fft.fft(y * hanning_window)
  xf = np.fft.fftshift(abs(xf))
  
  # gui yi hua
  #xf = (xf-xf.min())/(xf.max()-xf.min()) 
  xf = 20*np.log10(abs(xf)) - 154+35-2.5
  #xf = np.clip(xf, -100, 0)
  #xfp = 20*np.log10(np.clip(np.abs(xf), 1e-20, 1e100))
  
  skipNum = int(yLength * 100/1500)
  xf = xf[skipNum:yLength-skipNum]
  
  sampling_rate = 1.5*1e9
  fft_size = len(y)
  freqs = np.linspace(100*1e6, 1.4*1e9, fft_size-skipNum*2)  
  
  f_plot1.plot(freqs, xf)

  del xf
  
  #f.tight_layout()
  canvs.draw()

def sendcommandADCSAMPLE():
  sendcommand(0x5a04,0x0000,0x5a04,0x0000,0x0000,0x0000)
  
def sendcommandRDREG():
  sendcommand(0x5a01,0x0000,0xa501,0x0000,0x0000,0x0002)

def startCaptureThread():
  print  startCaptureThread.__name__
  
  statusVal.set(1)  
  global threadRunning
  threadRunning = True

  try:
    aiData = []
    aqData = []
    biData = []
    bqData = []
    
    while threadRunning:
      # Send command to get data
      
      sendcommandADCSAMPLE()
      udpSocketClient.receiveData()

      # Parse Data...
      data = udpSocketClient.mData
      
      print "Receive Total Length: ", len(data)
      if data:
        result = parseData(data, True)
        aiData = result[0]
        aqData = result[1]
        biData = result[2]
        bqData = result[3]
        
        try:
          if(channelVar.get() == 1):
            drawPicture(aiData,aqData)
          else:
            drawPicture(biData,bqData)
        except Exception,e:
            print "drawPicture Exception: ", e  
    
    # Save the last original payload
    if (saveLastData):
      #Save raw data
      now = datetime.datetime.now()
      currentTime = now.strftime('%Y-%m-%d-%H-%M-%S') 
      rawFileName = "RawIQ-" + currentTime + ".dat"
      rawFile=open(rawFileName,'wb')
      rawFile.write(data[12:])
      rawFile.close()
      
      #Save the IQ data 
      aiFileName = "AI-" + currentTime + ".txt"
      aqFileName = "AQ-" + currentTime + ".txt"
      biFileName = "BI-" + currentTime + ".txt"
      bqFileName = "BQ-" + currentTime + ".txt"
      aiFile=open(aiFileName,'wb')
      aqFile=open(aqFileName,'wb')
      biFile=open(biFileName,'wb')
      bqFile=open(bqFileName,'wb')
      
      # Save the IQ Data
      for i in range(len(aiData)):
         aiFile.write(str(aiData[i]))
         aiFile.write('\n');    
         aqFile.write(str(aqData[i]))
         aqFile.write('\n')
         biFile.write(str(biData[i]))
         biFile.write('\n');    
         bqFile.write(str(bqData[i]))
         bqFile.write('\n')

      aiFile.close()
      aqFile.close()
      biFile.close()
      bqFile.close()
    
    del aiData
    del aqData
    del biData
    del bqData
  
  except Exception,e:
    print "startCaptureThread exception: ", e
  # Reg status first then if it is success, continue to read data
  #sendcommandRDREG()
  #udpSocketClient.receiveData()
  
  # Parse Data...
  # data = udpSocketClient.mData
  # if data:
    # result = parseData(data, True)
    # statusValue = result[5]
    # if (statusValue & 1) != 1:
      # statusVal = 0
    # else:
      # statusVal = 1
      # # Send command to get data
      # sendcommandADCSAMPLE()
      # udpSocketClient.receiveData()

      # # Parse Data...
      # data = udpSocketClient.mData
      # if data:
        # result = parseData(data, True)
        # aiData = result[0]
        # aqData = result[1]
        # biData = result[2]
        # bqData = result[3]
        
        # if(channelVar.get() == 1):
          # drawPicture(aiData,aqData)
        # else:
          # drawPicture(biData,bqData)
          
          
def startCapture():
  print "Start Capture..."

  global threadRunning
  threadRunning = True
   
  global saveLastData
  saveLastData = False
  
  lRuntime = True # Runtime or read file
  
  try:
   if (lRuntime):
    thread.start_new_thread( startCaptureThread,() )
   else:
    thread.start_new_thread( readanddraw,() )
  except:
   print "Error: unable to start thread"
  
  
root = Tk()
root.wm_title('Signal Playback')

f = Figure(figsize=(5,4),dpi=100)
f_plot1 = f.add_subplot(111)
f_plot1.set_title('Signal')
f_plot1.set_xlabel('Freqs(HZ)')
f_plot1.set_ylabel('dBm')

udpSocketClient = UDPSocketClient()


canvs = FigureCanvasTkAgg(f,root)
canvs.show()
canvs.get_tk_widget().pack(side=TOP ,fill=BOTH ,expand =12)

frm = Frame(root)
#Top
frm_T = Frame(frm)

Label(frm_T,text='Link Status:').pack(padx=5, pady=10, side=LEFT)
statusVal = StringVar()
statusVal.set('0')
Entry(frm_T, textvariable =statusVal,relief=SUNKEN,width=10).pack(padx=5, pady=20, side=LEFT)

channelVar = IntVar()
channelVar.set(1)
Radiobutton(frm_T, text='Channel A', variable=channelVar, value=1).pack(padx=5, pady=10, side=LEFT)
Radiobutton(frm_T, text='Channel B', variable=channelVar, value=2).pack(padx=5, pady=10, side=LEFT)
Button(frm_T, text = 'Start Capture...',command = startCapture).pack(padx=5, pady=10, side=LEFT)
Button(frm_T, text = 'Stop Capture...',command = stopCapture).pack(padx=5, pady=10, side=LEFT)
frm_T.pack(side=TOP)

frm_B = Frame(frm)
#Button(root, text = 'Show',command = drawPicture).pack(padx=5, pady=30, side=LEFT) 
Label(frm_B,text='Register Address:').pack(padx=5, pady=10, side=LEFT)
Entry(frm_B, text = "0x02", relief=SUNKEN,width=10).pack(padx=5, pady=20, side=LEFT)
Label(frm_B,text='Register Value:').pack(padx=5, pady=10, side=LEFT)
Entry(frm_B, relief=SUNKEN,width=10).pack(padx=5, pady=20, side=LEFT)
Button(frm_B, text = 'Read Register',command = stopCapture).pack(padx=5, pady=10, side=LEFT) 
Button(frm_B, text = 'Write Register',command = stopCapture).pack(padx=5, pady=10, side=LEFT) 
frm_B.pack(side= TOP)

frm.pack()

zp1 = ZoomPan()
scale = 1.1
figZoom1 = zp1.zoom_factory(f_plot1, base_scale = scale)
figPan1 = zp1.pan_factory(f_plot1)

root.mainloop()
